/**
 * @file OdomData.h
 * @author  
 * @brief Odom数据类型
 * @version 1.0
 * @date 2021-03-23
 * 
 * @copyright Copyright (c) 2021
 * 
 */
 
#pragma once
#include <cstdint>
#include <Eigen/Eigen>
#include <cmath>
#include "tf2/utils.h"

namespace behavior_controller
{
    class OdomData
    {
    public:
        OdomData(){}
        OdomData(double &pos_x, double &pos_y, 
                 double &linear_speed_x,double &angular_speed_z, 
                 double &roll, double &pitch, double &yaw)
            : position_x_(pos_x), position_y_(pos_y), 
              linear_speed_x_(linear_speed_x),
              angular_speed_z_(angular_speed_z),yaw_(yaw){}

        OdomData(double &pos_x, double &pos_y, 
                 double &linear_speed_x,double &angular_speed_z, 
                 double x,double y,double z,double w)
            : position_x_(pos_x), position_y_(pos_y),
              linear_speed_x_(linear_speed_x),
              angular_speed_z_(angular_speed_z)
              {  set_rpy(x, y, z, w); }
        /**
         * @brief 获取roll的角度
         * @return
         */
        void set_rpy(double &yaw) 
        {
            yaw_= yaw;
        }

        void set_rpy(double &x, double &y, double &z,double &w) 
        {
            /*Eigen::Quaterniond  quaternion(w,x,y,z);
            Eigen::Vector3d eulerAngle = quaternion.matrix().eulerAngles(2,1,0); 
            // Z-Y-X，即RPY, 先绕x轴roll,再绕y轴pitch,最后绕z轴yaw 。        2,1,0= z,y,x =先沿x轴转再y再z
            //eulerAngle(0)=yaw(x) ,eulerAngle(1)=pitch(y)，eulerAngle(2)=roll(x)
            yaw_= eulerAngle(0);*/
            tf2::Quaternion quat = tf2::Quaternion(x, y, z, w);
            tf2::Matrix3x3(quat).getRPY(roll_, pitch_, yaw_);//进行转换
        }
        /**
         * @brief 获取x坐标
         * @return
         */
        double position_x() const { return position_x_; }
        /**
         * @brief 获取y坐标
         * @return
         */
        double position_y() const { return position_y_; }
        /**
         * @brief 获取yaw的角度
         * @return
         */
        double yaw() const { return yaw_; }
        double roll() const { return 0; }
        double pitch() const { return 0; }
        /**
         * @brief 获取x轴的线速度
         * @return
         */
        double vel_x() const { return linear_speed_x_; }

        /**
         * @brief 获取围绕z轴的角速度
         * @return
         */
        double vel_z() const { return angular_speed_z_; }
        double covariance_x()  const { return covariance_x_; }
        double covariance_y()  const { return covariance_y_; }
        double covariance_yaw()  const { return covariance_yaw_; }
        double covariance_linear_speed_x()  const { return covariance_linear_speed_x_; }
        double covariance_angular_speed_z()  const { return covariance_angular_speed_z_; }

    private:
        double position_x_;
        double position_y_;
        double yaw_,roll_,pitch_;  //航向角
        double linear_speed_x_;
        double angular_speed_z_;
        double covariance_x_;
        double covariance_y_;
        double covariance_yaw_;
        double covariance_linear_speed_x_;
        double covariance_angular_speed_z_;
    };
} // namespace behavior_controller